// #define __STDC_FORMAT_MACROS

#include <thread>
#include <iostream>
#include <fstream>

#include "rcs.hh"
#include "rcs_print.hh"
#include "nml_oi.hh"

#include "zucmodule.h"

#define LOCAL_SPINDLE_FORWARD (1)
#define LOCAL_SPINDLE_REVERSE (-1)
#define LOCAL_SPINDLE_OFF (0)
#define LOCAL_SPINDLE_INCREASE (10)
#define LOCAL_SPINDLE_DECREASE (11)
#define LOCAL_SPINDLE_CONSTANT (12)

static PyStructSequence_Field tool_fields[] = {
    {
        (char*)"id",
    },
    {
        (char*)"xoffset",
    },
    {
        (char*)"yoffset",
    },
    {
        (char*)"zoffset",
    },
    {
        (char*)"aoffset",
    },
    {
        (char*)"boffset",
    },
    {
        (char*)"coffset",
    },
    {
        (char*)"uoffset",
    },
    {
        (char*)"voffset",
    },
    {
        (char*)"woffset",
    },
    {
        (char*)"diameter",
    },
    {
        (char*)"frontangle",
    },
    {
        (char*)"backangle",
    },
    {
        (char*)"orientation",
    },
    {
        0,
    },
};

static PyStructSequence_Desc tool_result_desc = {(char*)"tool_result", /* name */
                                                 (char*)"",            /* doc */
                                                 tool_fields,
                                                 14};

static PyTypeObject ToolResultType;

#define ENUM(e) PyModule_AddIntConstant(m, const_cast<char*>(#e), e)
#define ENUMX(x, e) PyModule_AddIntConstant(m, x + const_cast<char*>(#e), e)

static struct PyModuleDef jkzuc_module = {.m_base = PyModuleDef_HEAD_INIT, .m_name = "jkzuc", .m_doc = NULL, .m_size = -1, .m_methods = {}};

PyMODINIT_FUNC PyInit_jkzuc(void)
{
    verbose_nml_error_messages = 0;
    clear_rcs_print_flag(~0);

    PyObject* m = NULL;
    PyObject* stat_obj = NULL;
    PyObject* cmd_obj = NULL;
    PyObject* err_obj = NULL;
    PyObject* ex_base_error = NULL;
    PyObject* robstat_obj = NULL;

    std::string versionStr;

    if (Stat_Ready() != 0)
    {
        return NULL;
    }

    if (Cmd_Ready() != 0)
    {
        return NULL;
    }

    if (Error_Ready() != 0)
    {
        return NULL;
    }

    m = PyModule_Create(&jkzuc_module);
    if (m == NULL)
    {
        goto HANDLE_ZUC_CREATE_FAILED;
    }
    // robstat_obj = RobotStat_CreateObject();
    extern PyTypeObject PyRobotStat_Type;
    Py_XINCREF(&PyRobotStat_Type);
    if (PyModule_AddObject(m, "robstat", (PyObject*)&PyRobotStat_Type) < 0)
    {
        goto HANDLE_ADD_STAT_OBJ_FAILED;
    }

    stat_obj = Stat_CreateObject();
    if (PyModule_AddObject(m, "stat", stat_obj) < 0)
    {
        goto HANDLE_ADD_STAT_OBJ_FAILED;
    }

    cmd_obj = Cmd_CreateObject();
    if (PyModule_AddObject(m, "command", cmd_obj) < 0)
    {
        goto HANDLE_ADD_CMD_OBJ_FAILED;
    }

    err_obj = Error_CreateObject();
    if (PyModule_AddObject(m, "error", err_obj) < 0)
    {
        goto HANDLE_ADD_ERR_OBJ_FAILED;
    }

    ex_base_error = Exception_Create();
    if (PyModule_AddObject(m, "BaseError", ex_base_error) < 0)
    {
        goto HANDLE_ADD_EXCEPTION_JKZUC_ERROR_FAILED;
    }

    PyModule_AddStringConstant(m, "PREFIX", ZUC_HOME);
    PyModule_AddStringConstant(m, "SHARE", ZUC_HOME "/share");
    PyModule_AddStringConstant(m, "nmlfile", ZUC_DEFAULT_NMLFILE);

    PyModule_AddIntConstant(m, "OPERATOR_ERROR", ZUC_OPERATOR_ERROR_TYPE);
    PyModule_AddIntConstant(m, "OPERATOR_TEXT", ZUC_OPERATOR_TEXT_TYPE);
    PyModule_AddIntConstant(m, "OPERATOR_DISPLAY", ZUC_OPERATOR_DISPLAY_TYPE);
    PyModule_AddIntConstant(m, "GRPC_RESPONSE_ERROR", GRPC_RESPONSE_ERROR_TYPE);
    PyModule_AddIntConstant(m, "GRPC_RESPONSE_WARNING", GRPC_RESPONSE_WARNING_TYPE);
    PyModule_AddIntConstant(m, "GRPC_RESPONSE_INFO", GRPC_RESPONSE_INFO_TYPE);
    PyModule_AddIntConstant(m, "NML_ERROR", NML_ERROR_TYPE);
    PyModule_AddIntConstant(m, "NML_TEXT", NML_TEXT_TYPE);
    PyModule_AddIntConstant(m, "NML_DISPLAY", NML_DISPLAY_TYPE);

    PyStructSequence_InitType(&ToolResultType, &tool_result_desc);
    PyModule_AddObject(m, "tool", (PyObject*)&ToolResultType);
    versionStr = ZUC_PACKAGE_VERSION;

    PyModule_AddObject(m, "controller_version", PyUnicode_FromString(versionStr.c_str()));
    PyModule_AddObject(m, "jkroot_dir", PyUnicode_FromString(ZUC_DEFAULT_ROOT_DIR));

    ENUMX(4, ZUC_LINEAR);
    ENUMX(4, ZUC_ANGULAR);

    ENUMX(9, ZUC_TASK_INTERP_IDLE);
    ENUMX(9, ZUC_TASK_INTERP_READING);
    ENUMX(9, ZUC_TASK_INTERP_PAUSED);
    ENUMX(9, ZUC_TASK_INTERP_WAITING);

    ENUMX(9, ZUC_TASK_MODE_MDI);
    ENUMX(9, ZUC_TASK_MODE_MANUAL);
    ENUMX(9, ZUC_TASK_MODE_AUTO);
    ENUMX(9, ZUC_TASK_MODE_DRAG);

    ENUMX(9, ZUC_TASK_STATE_OFF);
    ENUMX(9, ZUC_TASK_STATE_ON);
    ENUMX(9, ZUC_TASK_STATE_POWERED_OFF);
    ENUMX(9, ZUC_TASK_STATE_POWERED_ON);

    ENUMX(6, LOCAL_SPINDLE_FORWARD);
    ENUMX(6, LOCAL_SPINDLE_REVERSE);
    ENUMX(6, LOCAL_SPINDLE_OFF);
    ENUMX(6, LOCAL_SPINDLE_INCREASE);
    ENUMX(6, LOCAL_SPINDLE_DECREASE);
    ENUMX(6, LOCAL_SPINDLE_CONSTANT);

    ENUMX(6, LOCAL_JOG_STOP);
    ENUMX(6, LOCAL_JOG_CONTINUOUS);
    ENUMX(6, LOCAL_JOG_INCREMENT);
    ENUMX(6, LOCAL_JOG_ABS);

    ENUMX(6, LOCAL_AUTO_RUN);
    ENUMX(6, LOCAL_AUTO_PAUSE);
    ENUMX(6, LOCAL_AUTO_RESUME);
    ENUMX(6, LOCAL_AUTO_STEP);

    ENUMX(4, ZUC_TRAJ_MODE_FREE);
    ENUMX(4, ZUC_TRAJ_MODE_COORD);
    ENUMX(4, ZUC_TRAJ_MODE_TELEOP);
    ENUMX(4, ZUC_TRAJ_MODE_TELEOP_TOOL);
    ENUMX(4, ZUC_TRAJ_MODE_DRAG);
    ENUMX(4, ZUC_TRAJ_MODE_SERVOJOP);
    ENUMX(4, ZUC_TRAJ_MODE_ADMITTANCE);

    ENUMX(4, ZUC_MOTION_TYPE_TRAVERSE);
    ENUMX(4, ZUC_MOTION_TYPE_FEED);
    ENUMX(4, ZUC_MOTION_TYPE_ARC);
    ENUMX(4, ZUC_MOTION_TYPE_TOOLCHANGE);
    ENUMX(4, ZUC_MOTION_TYPE_PROBING);
    ENUMX(4, ZUC_MOTION_TYPE_INDEXROTARY);
    ENUMX(4, ZUC_MOTION_TYPE_JOINT_LINE);

    ENUM(KINEMATICS_IDENTITY);
    ENUM(KINEMATICS_FORWARD_ONLY);
    ENUM(KINEMATICS_INVERSE_ONLY);
    ENUM(KINEMATICS_BOTH);

    ENUMX(4, ZUC_DEBUG_CONFIG);
    ENUMX(4, ZUC_DEBUG_VERSIONS);
    ENUMX(4, ZUC_DEBUG_TASK_ISSUE);
    ENUMX(4, ZUC_DEBUG_NML);
    ENUMX(4, ZUC_DEBUG_MOTION_TIME);
    ENUMX(4, ZUC_DEBUG_INTERP);
    ENUMX(4, ZUC_DEBUG_RCS);
    ENUMX(4, ZUC_DEBUG_INTERP_LIST);

    ENUMX(9, ZUC_TASK_EXEC_ERROR);
    ENUMX(9, ZUC_TASK_EXEC_DONE);
    ENUMX(9, ZUC_TASK_EXEC_WAITING_FOR_MOTION);
    ENUMX(9, ZUC_TASK_EXEC_WAITING_FOR_MOTION_QUEUE);
    ENUMX(9, ZUC_TASK_EXEC_WAITING_FOR_IO);
    ENUMX(9, ZUC_TASK_EXEC_WAITING_FOR_MOTION_AND_IO);
    ENUMX(9, ZUC_TASK_EXEC_WAITING_FOR_DELAY);
    ENUMX(9, ZUC_TASK_EXEC_WAITING_FOR_SYSTEM_CMD);
    ENUMX(9, ZUC_TASK_EXEC_WAITING_FOR_SPINDLE_ORIENTED);

    ENUMX(7, ZUCMOT_MAX_JOINTS);
    ENUMX(7, ZUCMOT_MAX_AXIS);

    ENUM(RCS_DONE);
    ENUM(RCS_EXEC);
    ENUM(RCS_ERROR);

    return m;

HANDLE_ADD_EXCEPTION_JKZUC_ERROR_FAILED:
    Exception_Destory();

HANDLE_ADD_ERR_OBJ_FAILED:
    Error_DestroyObject(err_obj);

HANDLE_ADD_CMD_OBJ_FAILED:
    Cmd_DestroyObject(cmd_obj);

HANDLE_ADD_STAT_OBJ_FAILED:
    Stat_DestroyObject(stat_obj);

HANDLE_ZUC_CREATE_FAILED:
    Py_CLEAR(m);

    return NULL;
}
